# -- coding:utf-8 --
from lib.MiniBotControlLib import MiniBotControlLib
import time
import math


def msSleep(ms):
    time.sleep(0.001 * ms)

class CarRobot:
    #******************定义用户参数******************
    W_speed = 80  #传入舵机速度，1（MIN）至100（MAX）
    W_angle = 90  #传入小车自转角度，1（MIN）至360（MAX）
    PI = 3.14159
    Speed_Round = 40
    
    #******************定义硬件参数******************/
    Diam_Rub_Wheel = 70 #橡胶轮的直径mm #原70，实测66
    Diam_All_Wheel = 80 #全向轮的直径mm
    Diam_Mai_Wheel = 80 #麦克纳姆轮的直径mm
    
    Rad_two_wheel = 160    #二轮的整车半径mm
    Rad_three_wheel = 160  #三轮的整车半径mm
    Rad_FourA_wheel = 160  #四向轮整车半径mm
    Rad_FourM_wheel = 160  #麦轮的整车半径mm
    Rad_FourB_wheel = 160  #四并轮整车半径mm
    Rad_Car_R = 160        #小车（所有轮）整车半径mm
    
    #小车一圈的周长
    Two_Circle = (2 * PI * Rad_two_wheel)
    Thr_Circle = (2 * PI * Rad_three_wheel)
    FourA_Circle = (2 * PI * Rad_FourA_wheel)
    FourM_Circle = (2 * PI * Rad_FourM_wheel)
    FourB_Circle = (2 * PI * Rad_FourB_wheel)
    
    #轮子一圈的周长
    Rub_Circle = (PI * Diam_Rub_Wheel)
    All_Circle = (PI * Diam_All_Wheel)
    Mai_Circle = (PI * Diam_Mai_Wheel)
    
    #小车周长与轮子周长的比例,即轮子要走几圈，小车才走完一圈
    Two_Scale = (Two_Circle / Rub_Circle)
    Thr_Scale = (Thr_Circle / All_Circle)
    FourA_Scale = (FourA_Circle / All_Circle)
    FourM_Scale = (FourM_Circle / Mai_Circle)
    FourB_Scale = (FourB_Circle / All_Circle)
    
    One_Round_Time = 3160 #I模块在40的速度下转一圈估计的时间ms，速度暂时固定40
    
    #小车旋转1°对应的i模块旋转时间ms
    Two_Ms = (Two_Scale * One_Round_Time / 360.0) #小车自转一周耗费的时间/360°，可得出转一度需要多少ms
    Thr_Ms = (Thr_Scale * One_Round_Time / 360.0)
    FourA_Ms = (FourA_Scale * One_Round_Time / 360.0)
    FourM_Ms = (FourM_Scale * One_Round_Time / 360.0)
    FourB_Ms = (FourB_Scale * One_Round_Time / 360.0)
  
    #标记运行状态
    isRunning = False

    #ID列表
    wheel_id1 = 0
    wheel_id2 = 0
    wheel_id3 = 0
    wheel_id4 = 0
    Mode = 2

    #控制Api
    #lib = MiniBotControlLib("udp_mode")
    def __init__(self,robotBase):
        self.robotBase = robotBase
        self.lib = self.robotBase.mclib

    #绑定ID与模型:ids F,B,l1,l2,l3,l4,r1,r2,r3,r4
    def BindIds(self,ids,mode):
        self.Mode = mode
        if(self.Mode == 2):
            self.wheel_id1 = ids[3]
            self.wheel_id2 = ids[7]
        if(self.Mode == 3):
            self.wheel_id1 = ids[0]
            self.wheel_id2 = ids[4]
            self.wheel_id3 = ids[8]
            self.wheel_id4 = 0
        if(self.Mode == 4):
            self.wheel_id1 = ids[3]
            self.wheel_id2 = ids[0]
            self.wheel_id3 = ids[7]
            self.wheel_id4 = ids[1]
        if(self.Mode == 5 or mode == 6):
            self.wheel_id1 = ids[2]
            self.wheel_id2 = ids[6]
            self.wheel_id3 = ids[5]
            self.wheel_id4 = ids[9]

        self.lib.SetMotorMode(self.wheel_id1,1)
        self.lib.SetMotorMode(self.wheel_id2,1)
        self.lib.SetMotorMode(self.wheel_id3,1)
        self.lib.SetMotorMode(self.wheel_id4,1)

    def SetWheelSpeed(self,id,speed):
        self.lib.SetRunningSpeed(id,int(speed))
        #self.lib.SetMotoSpeedMicroPercent(id,speed * 10)

    #************* 两轮 - Start

    def GoForward_2wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        pass

    def GoBack_2wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        pass

    def TurnLeft_2wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        pass

    def TurnRight_2wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        pass

    #************* 两轮 - End



    #************* 三轮 - Start

    def GoForward_3wheel(self):
        self.SetWheelSpeed(self.wheel_id1,0)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        pass

    def GoBack_3wheel(self):
        self.SetWheelSpeed(self.wheel_id1,0)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        pass

    def TurnLeft_3wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        pass

    def TurnRight_3wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        pass

    #************* 三轮 - End



    #************* 四向轮 - Start

    def GoForward_4wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,0)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,0)
        pass

    def GoBack_4wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,0)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,0)
        pass

    def TurnLeft_4wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed * -1)
        pass

    def TurnRight_4wheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed)
        pass

    #************* 四向轮 - End



    #************* 四麦轮 - Start

    def GoForward_4Mwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed * -1)
        pass

    def GoBack_4Mwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed)
        pass

    def TurnLeft_4Mwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed * -1)
        pass

    def TurnRight_4Mwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed)
        pass

    def RunLeft_4Mwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed)
        pass

    def RunRight_4Mwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed * -1)
        pass

    #************* 四麦轮 - End




    #************* 四并轮 - Start

    def GoForward_4Nwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed * -1)
        pass

    def GoBack_4Nwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed)
        pass

    def TurnLeft_4Nwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed * -1)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed * -1)
        pass

    def TurnRight_4Nwheel(self):
        self.SetWheelSpeed(self.wheel_id1,self.W_speed)
        self.SetWheelSpeed(self.wheel_id2,self.W_speed)
        self.SetWheelSpeed(self.wheel_id3,self.W_speed)
        self.SetWheelSpeed(self.wheel_id4,self.W_speed)
        pass

    #************* 四麦轮 - End



    #************* 旋转指定角度 - Start

    def Rotate_2wheel(self,angle):
        if(angle > 0):
            self.TurnLeft_2wheel()
        else:
            self.TurnRight_2wheel()

        sl = self.Two_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.Stop()
        pass

    def Rotate_3wheel(self,angle):
        if(angle > 0):
            self.TurnLeft_3wheel()
        else:
            self.TurnRight_3wheel()

        sl = self.Thr_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.Stop()
        pass

    def Rotate_4wheel(self,angle):
        if(angle > 0):
            self.TurnLeft_4wheel()
        else:
            self.TurnRight_4wheel()

        sl = self.FourA_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.Stop()
        pass

    def Rotate_4Mwheel(self,angle):
        if(angle > 0):
            self.TurnLeft_4Mwheel()
        else:
            self.TurnRight_4Mwheel()

        sl = self.FourM_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.Stop()
        pass

    def Rotate_4Nwheel(self,angle):
        if(angle > 0):
            self.TurnLeft_4Nwheel()
        else:
            self.TurnRight_4Nwheel()

        sl = self.FourB_Ms * angle * 0.5
        #等待以让硬件旋转指定时间以完成目标角度
        msSleep(sl)
        self.Stop()
        pass

    #************* 旋转指定角度 - End


    def Stop_wheel(self):
        self.SetWheelSpeed(self.wheel_id1,0)
        self.SetWheelSpeed(self.wheel_id2,0)
        self.SetWheelSpeed(self.wheel_id3,0)
        self.SetWheelSpeed(self.wheel_id4,0)
        pass


    #************* 全局控制 - Start

    def Stop(self):
        self.isRunning = False
        self.Stop_wheel()

    def GoForward(self):
        self.isRunning = True
        if(self.Mode == 2):
            self.GoForward_2wheel()
        if(self.Mode == 3):
            self.GoForward_3wheel()
        if(self.Mode == 4):
            self.GoForward_4wheel()
        if(self.Mode == 5):
            self.GoForward_4Mwheel()
        if(self.Mode == 6):
            self.GoForward_4Nwheel()
        pass

    def GoBack(self):
        self.isRunning = True
        if(self.Mode == 2):
            self.GoBack_2wheel()
        if(self.Mode == 3):
            self.GoBack_3wheel()
        if(self.Mode == 4):
            self.GoBack_4wheel()
        if(self.Mode == 5):
            self.GoBack_4Mwheel()
        if(self.Mode == 6):
            self.GoBack_4Nwheel()
        pass

    def TurnLeft(self):
        self.isRunning = True
        if(self.Mode == 2):
            self.TurnLeft_2wheel()
        if(self.Mode == 3):
            self.TurnLeft_3wheel()
        if(self.Mode == 4):
            self.TurnLeft_4wheel()
        if(self.Mode == 5):
            self.TurnLeft_4Mwheel()
        if(self.Mode == 6):
            self.TurnLeft_4Nwheel()
        pass

    def TurnRight(self):
        self.isRunning = True
        if(self.Mode == 2):
            self.TurnRight_2wheel()
        if(self.Mode == 3):
            self.TurnRight_3wheel()
        if(self.Mode == 4):
            self.TurnRight_4wheel()
        if(self.Mode == 5):
            self.TurnRight_4Mwheel()
        if(self.Mode == 6):
            self.TurnRight_4Nwheel()
        pass

    def RunLeft(self):
        if(self.Mode == 5):
            self.isRunning = True
            self.RunLeft_4Mwheel()
        pass

    def RunRight(self):
        if(self.Mode == 5):
            self.isRunning = True
            self.RunRight_4Mwheel()
        pass

    def Rotate(self,angle):
        self.isRunning = True
        if(self.Mode == 2):
            self.Rotate_2wheel(angle)
        if(self.Mode == 3):
            self.Rotate_3wheel(angle)
        if(self.Mode == 4):
            self.Rotate_4wheel(angle)
        if(self.Mode == 5):
            self.Rotate_4Mwheel(angle)
        if(self.Mode == 6):
            self.Rotate_4Nwheel(angle)
        pass

    #************* 全局控制 - End



    #************* For ROS DRIVER - Start

    #设置速度（米/秒）
    def SetRunningSpeedWithMS(self,ms):

        #I空载 最大速度为3400/S
        MAX_STEP = 3400
        #ms = ms * 1.25
        #MAX_STEP=3050 #负载观察值

        #获取轮子周长
        c = self.Rub_Circle  #橡胶轮
        if(self.Mode == 5):
            c = self.Mai_Circle   #麦克朗姆轮
        if(self.Mode == 3 or self.Mode == 4):
            c = self.All_Circle   #麦克朗姆轮

        #计算目标圈数
        mms = ms * 100 * 10.0
        qcount = mms / c

        #计算每秒步数
        #I模块一圈为4095,空载最大步数速度为3400
        setps = qcount * 4095
        validSetp = min(abs(setps),MAX_STEP)
        speedPercent = validSetp * 100.0 / MAX_STEP

        maxSpeed = MAX_STEP / 4096.0 * c / 1000
        print("Max speed is:" + str(maxSpeed) + "m/s")
        print("Now is setting speed (" + str(speedPercent) + "% steps=" + str(validSetp) + "):" + str(maxSpeed * speedPercent / 100) + "m/s")
        if(ms < 0):
           speedPercent = -speedPercent

        self.W_speed = int(speedPercent)
        return
    
    
     #设置速度（弧度/秒）
     #最大值：(3400/4095*70*3.14)/(160*3.14*2)*(2*3.14)=1.1405982905983
    def SetRunningSpeedWithRadS(self,rads):

        #I空载 最大速度为3400/S
        MAX_STEP = 3400
        
        #获取轮子周长
        wheelc = self.Rub_Circle  #橡胶轮
        if(self.Mode == 5):
            wheelc = self.Mai_Circle   #麦克朗姆轮
        if(self.Mode == 3 or self.Mode == 4):
            wheelc = self.All_Circle   #全向轮

        #获取整车周长
        c = self.Rad_Car_R * self.PI * 2 

        #每秒需走多少周长
        s = rads / (2 * self.PI) * c

        #转为即每秒需走多少圈
        rounds = s / wheelc

        #转换为每秒需走多少步
        steps = rounds * 4095

        #校正最步数值
        validSetp = min(abs(steps),MAX_STEP) #*2为实际表现系数
        speedPercent = validSetp * 100.0 / MAX_STEP

        self.W_speed = int(speedPercent)
        return;

    
    #************* For ROS DRIVER - End

